#ifndef _SIUT_SIMD_HPLANE_HPP_
#define _SIUT_SIMD_HPLANE_HPP_

#ifndef SIUT_FLOAT_EPSILON
#define SIUT_FLOAT_EPSILON 0.001
#endif

#include "Vec3f.hpp"
#include "Vec4f.hpp"
#include "BBox3f.hpp"

namespace siut
{
namespace simd
{
namespace standard
{

 
  struct HPlane3f
  {
    Vec3f normal_;
    float offset_;
    
    inline HPlane3f()
    {
      normal_ = Vec3f(0.f, 0.f, 0.f);
      offset_ = 0.f;
    }
#ifndef NVCC
    HPlane3f(const Vec3f &v0, const Vec3f &v1, const Vec3f &v2)
    {
      normal_ = cross(v1 - v0, v2 - v0);
      offset_ = -dot(normal_, v0);      
    }

    HPlane3f(float a, float b, float c, float d)
    {
      normal_[0] = a;
      normal_[1] = b;
      normal_[2] = c;
      offset_ = d;
    }

    HPlane3f(const Vec4f &v)
    {
      normal_[0] = v[0];
      normal_[1] = v[1];
      normal_[2] = v[2];
      offset_ = v[3];
    }
#endif    
  };

//forward declaration
  inline HPlane3f createHPlane(const Vec3f &p0, const Vec3f &p1, const Vec3f &p2);
  inline HPlane3f createHPlane(Vec3f &normal, float offset);
  inline HPlane3f createHPlane(float a, float b, float c, float d);
  inline Vec3f toVector(HPlane3f orig);
  inline Vec3f intersection(const HPlane3f &hp1, const HPlane3f &hp2, const HPlane3f &hp3);
  inline HPlane3f normalize(HPlane3f &plane);
  inline float distance(const HPlane3f &plane, Vec3f &vec);
  inline int inside(const HPlane3f &plane, const Vec3f &point);
  inline bool insideFast(const HPlane3f &plane, const Vec3f point);
  inline const Vec3f getNormal(const HPlane3f &plane);
  inline float getOffset(const HPlane3f &plane);
  inline void setOffset(HPlane3f &plane, float offset);
  inline void moveTo(HPlane3f &plane, const Vec3f &point);
  inline float distance(const HPlane3f &plane, BBox3f &bbox);
  inline std::ostream& operator << (std::ostream &os, const HPlane3f &plane);


 
  //implementation
  inline HPlane3f createHPlane(const Vec3f &p0, const Vec3f &p1, const Vec3f &p2)
  {
    HPlane3f temp;
    temp.normal_ = cross(p1 - p0, p2 - p0);
    moveTo(temp, p0);
    return temp;
  }

  inline HPlane3f createHPlane(Vec3f &normal, float offset)
  {
    HPlane3f temp;
    temp.normal_ = normal;
    temp.offset_ = offset;
    return temp;
  }

  inline HPlane3f createHPlane(float a, float b, float c, float d)
  {
    HPlane3f temp;
    temp.normal_[0] = a;
    temp.normal_[1] = b;
    temp.normal_[2] = c;
    temp.offset_ = d;

    return temp;
  }

  inline Vec4f toVector(const HPlane3f &orig)
  {
    return Vec4f(orig.normal_[0], orig.normal_[1], orig.normal_[2], orig.offset_);
  }

  inline Vec3f intersection(const HPlane3f &hp1, const HPlane3f &hp2, const HPlane3f &hp3)
  {
    float trip3d = triple3d(hp1.normal_, hp2.normal_, hp3.normal_);
    trip3d = 1.f / trip3d;

    Vec3f num = (-hp1.offset_ * cross(hp2.normal_, hp3.normal_),
		 -hp2.offset_ * cross(hp3.normal_, hp1.normal_),
		 -hp3.offset_ * cross(hp1.normal_, hp2.normal_));
    return num * trip3d;
  }

  inline HPlane3f normalize(HPlane3f &plane)
  {
    float temp = 1.f/length(plane.normal_);
    plane.normal_ *= temp;
    plane.offset_ *= temp;
    return plane;
  }

  inline float distance(const HPlane3f &plane, Vec3f &vec)
  {
    return plane.normal_.dot(vec) + plane.offset_;
  }

  inline int inside(const HPlane3f &plane, const Vec3f &point)
  {
    float d = distance(plane, (Vec3f&)point);
    if(d > SIUT_FLOAT_EPSILON)
      {
	return 1;
      }
    else if (d < SIUT_FLOAT_EPSILON)
      {
	return -1;
      }
    else
      {
	return 0;
      }
  }

  inline bool insideFast(const HPlane3f &plane, const Vec3f point)
  {
    return distance(plane, (Vec3f&) point) >= 0.0;
  }

  inline const Vec3f getNormal(const HPlane3f &plane)
  {
    return plane.normal_;
  }

  inline float getOffset(const HPlane3f &plane)
  {
    return plane.offset_;
  }

  inline void setOffset(HPlane3f &plane, float offset)
  {
    plane.offset_ = offset;
  }

  inline void moveTo(HPlane3f &plane, const Vec3f &point)
  {
    plane.offset_ = -dot(plane.normal_, point);
  }

  inline float distance(const HPlane3f &plane, BBox3f &bbox)
  {
    Vec3f vCorner;
    corner(bbox, 0, vCorner);
    float dist = distance(plane,vCorner);
    for(int i = 1; i < 3; ++i)
      {
	corner(bbox, i, vCorner);
	float dist2 = distance(plane, vCorner);
	if(dist*dist2 < 0.0)
	  {
	    dist = 0.0; //intersection
	  }
	dist = dist2 * dist2 < dist * dist ? dist2 : dist;
      }
    return dist;
  }

  inline std::ostream& operator << (std::ostream &os, const HPlane3f &plane)
    {
      os << "normal = " << plane.normal_ << " offset = " << plane.offset_ << std::endl;
      return os;
    }
    

}//end namespaces
}
}

#endif
